EKF2: Enable simultaneous optical flow and GPS use #4933
Conversation
Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained.
|
This is great Paul! The PR looks good to me, but let me point some places that might need to be modified too:
|
|
@OXINARF in response to your review comments:
|
priseborough
force-pushed
the
pr-ekfOptFlowGps
branch
from
2c75aa5
to
775a12a
Compare
|
With the last commit 775a12a I am able to get the EKF to go into relative position mode at the start before getting GPS, but without GPS it won't let me arm in LOITER or select LOITER mode after arming, so further investigation is required. |
|
Awesome job Paul! Been a long time in the works! |
PR - agree, will fix.
PR - thats easy because there is a flowDataValid flag that indicates the presence of fresh flow measurements
PR - Ideally should if the distance to ground is less than a reasonable minimum focal distance, eg 50cm. We do want it to be possible to pick up the copter whilst disarmed and walk it around to both test the flow operation and also to be able to reposition it without the zeroed flow measurements fighting the GPS data.
PR - If the logic used to zero the flow measurements is reworked to do so if optical flow takeoff is not detected and height above ground is less than 0.5m, then I believe it will address both points 4) and 5)
I believe this is a pre-arm check problem. The Loiter mode is registered has needing GPS and so it won't let you arm until the GPS has passed all pre-arm checks (including being good enough for the EKF). But you should be able to arm in a non-GPS mode (Stabilize, AltHold) and then change to Loiter. PR - agree |
Terrain height is relevant whenever optical flow data is present
|
These patches address the review comments and have been test flown without a 3D GPS lock verifying that with ARMING_CHECKS=-9 the vehicle could be armed and flown in LOITER mode. |
|
@priseborough Sorry for being a pain, but I'm not entirely convinced in the last case. Let's suppose that we have:
Shouldn't the detectOptFlowTakeoff function run in this case? My though is that it would run when you exclusively use optical flow (because otherwise we wouldn't have a way to tell we took off - maybe this is where I'm wrong?). |
…data A specialised takeoff check is now always performed when we receive new flow data as the default behaviour is to try and use flow data whenever it is received, rather than limit its use to a use to a flow-only mode of operation that had to be selected via user parameter.
|
The (terrainState - stateStruct.position.z) < 0.5f) check in SelectFlowFusion will still operate (as it did during my testing), but I think it would be better to always run the specialised optical flow takeoff check when we receive flow data as it provides an earlier detection. I will add a patch that removes the condition so that detectOptFlowTakeoff() always gets called when new flow data arrives. |
|
Merged! Thanks very much @OXINARF for the review. |
|
Hello Paul, I make flight with iris and SF11 + optical flow flying in loiter mode in AC3.5RC1 and standard ek2 settings. During flight I test to go from no gps reception zone to gps zones, also, I flight over the ground and passing over roof of disused industrial building for see how ekf handle straight range finder changes. Could you please check the logs and comment what you see and mayby tell us how to tune EK2 gates and noises for improve position holding and EK2 gps lidar optflow fuse? Thanks you. |
|
@roque-canales I suggest using EKF3 if you're doing EKF stress testing.
…On Fri, Mar 3, 2017 at 6:09 AM, roque-canales ***@***.***> wrote:
Hello Paul,
I make flight with iris and SF11 + optical flow flying in loiter mode in
AC3.5RC1 and standard ek2 settings.
During flight I test to go from no gps reception zone to gps zones, also,
I flight over the ground and passing over roof of disused industrial
building for see how ekf handle straight range finder changes.
Could you please check the logs and comment what you see and mayby tell us
how to tune EK2 gates and noises for improve position holding and EK2 gps
lidar optflow fuse?
Thanks you.
—
You are receiving this because you are subscribed to this thread.
Reply to this email directly, view it on GitHub
<#4933 (comment)>,
or mute the thread
<https://github.com/notifications/unsubscribe-auth/AEj7GwcTz88evp9_KuGzSxGMw2s-9S5Uks5riB7_gaJpZM4KOPbP>
.
|
|
@roque-canales Given that this is a log analysis request, can you please post your query here http://discuss.ardupilot.org/c/log-analysis-and-tuning/log-analysis-copter |
|
Ok Magicrub. Ok Paul, thank you. I just see that Randy found that EKF no recover position after glitch on AC3.5. I experienced this issue on another AC3.5 flight. When a glitch occur, drone start to drift, and no recover, even if gps back to g |
|
EK3-CHALLENGING-ENVIRONEMENT-FLOW-SF11-GPSGLITCH.zip I started from non gps zone, perfect, At the end of flight I go back to non gps zone, and EKF crashed i think, iris go to max lean angle, I switch to stabilise for regain control. (iris is robust and perfect for this tests. :)) It is like EK3 don't want to reject gps bad readings.... Maybe this issue just need to tune gate ore noise for gps? And if it's the case, could you tell me new value for remake test flight? |
|
@roque-canales Please raise your question here http://discuss.ardupilot.org/c/log-analysis-and-tuning/log-analysis-copter This github issue is closed |
|
ok sorry |
Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained.
Parameter Settings
The EK2_RNG_USE_HGT was set to 50 so that below 50% of the range finders max range of 40m and when the vehicle is stationary, the range finder height is used as the primary EKF height reference. This is the preferred option for combined GPS and optical flow operation.
The EK2_GPS_TYPE was set to 0. Previously this would mean that optical flow data would be ignored, but now optical flow data will be used if a sensor is fitted and enabled by the FLOW_ENABLE parameter. Setting EK2_GPS_TYPE = 3 will still have the same effect as before, putting the EKF into an optical flow only mode that ignores GPS
Ground Testing
The ability to lose and regain GPS has been tested on the ground. The was conducted inside to provide a very marginal GPS signal. Placing a hand over the antenna was able to induce complete loss of GPS satellite tracking resulting in a switch to optical flow/relative position mode. When the GPS blockage was removed and the 3D lock recovered, the EKF reset its position to the GPS and switched to GPS mode. This was repeated twice.
GPS position accuracy - very poor and rises rapidly when 3D lock is lost
GPS position innovations - flat-lining indicates GPS is not being fused
Optical flow innovations - flow data was fused throughout
NE Position - note the position resets when GPS lock is regained.
Flight Testing
This branch has been flight tested on a 3DR Iris airframe with a PX4Flow v1.3 optical flow sensor and a Lightware SF10B range finder over flat terrain (sports field). The GPS was getting interference from the optical flow sensor with high levels of noise. The flight log can be found here: https://drive.google.com/file/d/0By4v2BuLAaCfeWtyd2xQTWxXZEE/view?usp=sharing
GPS reported speed error (m/s)
GPS reported position error (m)
Despite this, the copters position hold was steady throughout. The flight went through a range of speeds up to 5 m/s and altitudes up to 39m.
Optical flow innovations remained good throughout.
Optical flow innovations (mrad/sec)
GPS position and velocity innovations were high, reflecting the amount of interference. Normally GPS innovations this high would result in an unsteady position hold, but the optical flow fusion reduced the effect of the GPs errors on position and velocity output.
GPS position innovations (m)
GPS velocity innovations (m/s)
The automatic switching between range finder and Baro height can be seen in the height innovations. The periods of increased noise correspond to use of the Baro.
